/*圆环数据
 * 右圆环
 *              |         —             —                       —            |
 *   入                                                                 >3000                                                                 4095
 *   出                   4095                                     >2500
 * 左圆环
 *              |         —             —                       —            |
 * 入                    4095                                       >3000
 *  出                                                                   >3000                                                                 4095
 *
 * 直角数据
 * 直角方向对应的电感会超过在知道读取的最大值，中间电感值会减小（最大不超过2000）
 *  十字数据
 *  中间电感值最大2300 最小2000  最两侧电感都为4095
 *  判断顺序：1.十字 2.圆环 3.直角 4.pid跑
 *
 */
#include "ANO_DT.h"
#include "SEEKFREE_FUN.h"
#include "isr.h"
#include"liu_chabihe.h"
#include"zf_vadc.h"
#include "SEEKFREE_FUN.h"
#include<liu_motor.h>
#include "zf_gtm_pwm.h"
#include "SEEKFREE_OLED.h"
#include "SEEKFREE_WIRELESS.h"
#include "zf_stm_systick.h"
#include"liu_key.h"

float data_adc[N][M];                       //ADC要用的
float data_sum[N];                          //临时存放和
float max_adc_data[N],min_adc_data[N];
float av_adc[N];                            //采集的M次ADC值得平均
float need_one[N];                          //处理后最终得到的值
float need;
float max[N],min[N];
uint16 ting;
uint16 Indu_LL;
uint16 Indu_LM;
uint16 Indu_L;
uint16 Indu_R;
uint16 Indu_RM;
uint16 Indu_RR;
uint16 Indu_M;
uint16 tingche_adc;
uint8 pid_flagbit = 0;
uint8 m;
uint8 left_flag;
uint8 right_flag;
uint8 zuo_enterring=0;//进环标志位 0禁止入环 1可以入环
uint8 you_enterring=0;
uint8 outring = 0;//出环标志位
uint8 yuansu_flag; //判断元素
uint8 speed4;
//uint8 lrring = 0;//判断左侧环右侧环  0默认  1左侧环 2 右侧环
uint8 huan_flag = 0;//用来判断入环结束 默认 0   左侧环 1  右侧环  2
uint16 huanchu_flag;//出环 标志位
float electricity_P;        //方向控制p
float electricity_D;        //方向控制d

float elec_redu_in,elec_add_in;//方向偏差（一对水平电感的差比和偏差）
float elec_redu_out,elec_add_out;//方向偏差（一对垂直电感的差比和偏差）
float cabihe_in,chabihe_out;
float g_Turn_Error[2][2];         //方向偏差     (g_Turn_Error_Temp1[0]为一对水平电感的差比和偏差)
//             (g_Turn_Error_Temp1[1]为一对垂直电感的差比和偏差)
float g_Turn_Error_dot[2];           //方向偏差微分  (g_fDirectionError_dot[0]为一对水平电感的差比和偏差微分)
//             (g_fDirectionError_dot[1]为一对垂直电感的差比和偏差微分)
float Object_elec_Turn_Out;
float Object_elec_Turn_Out_1;
float Object_elec_Turn_Out_2;
float elec_Turn_Out;
float display_chabihe,display_dot;
uint8 zhijiao_flag;
int16 ai_elec_turn_out;
int8 ad_data[7]; //ad数据 回传数据用 不跑ai不用
uint8 ai_data_flag = 0;// ad数据采集完成标志位  1ad数据采集完成   0ad数据采集未完成
uint8 ai_send_buff[11];
uint8 virsco_data[10];
uint16 ad_data1[7];
uint16 ad_min[4];
uint16 ad_max[4];
uint16 podao_flag;

uint8 zhijiao_flag1;
sint32 zhijiao_maichong;
uint16 lang_pwm =3600;
uint16 wan_pwm =3000;
float electricity_P_z;
float electricity_D_z;
float electricity_P_w;
float electricity_D_w;
float display_chabihe;
void data_treating_direct(void)
{
    Indu_LL = adc_mean_filter(ADC_0,ADC0_CH0_A0,ADC_12BIT,10);
    Indu_LM = adc_mean_filter(ADC_0,ADC0_CH2_A2,ADC_12BIT,10);
    Indu_L =  adc_mean_filter(ADC_0,ADC0_CH1_A1,ADC_12BIT,10);
    Indu_R =  adc_mean_filter(ADC_0,ADC0_CH5_A5,ADC_12BIT,10);
    Indu_RM = adc_mean_filter(ADC_0,ADC0_CH4_A4,ADC_12BIT,10);
    Indu_RR = adc_mean_filter(ADC_0,ADC0_CH6_A6,ADC_12BIT,10);
    Indu_M  = adc_mean_filter(ADC_0,ADC0_CH3_A3,ADC_12BIT,10);
    tingche_adc =  adc_mean_filter(ADC_1,ADC1_CH2_A10,ADC_12BIT,10);
    if(Indu_M <1800)
    {
        zhijiao_flag = 1;
        ting=1;
    }
    else
        zhijiao_flag = 0;
    //    if(Indu_M >3500)
    //    {
    //        podao_flag =1;
    //    }
    //    else
    //    {
    //        podao_flag = 0;
    //    }
    /*   if (Indu_LL < ad_min[0])
        ad_min[0] =Indu_LL;     // 刷新最小值
    else if (Indu_LL > ad_max[0])
        ad_max[0] = Indu_LL;     // 刷新最大值

    if (Indu_RR < ad_min[1])
        ad_min[1] = Indu_RR;
    else if (Indu_RR > ad_max[1])
        ad_max[1] = Indu_RR;

    if (Indu_L < ad_min[2])
        ad_min[2] =Indu_L;     // 刷新最小值
    else if (Indu_L > ad_max[2])
        ad_max[2] = Indu_L;     // 刷新最大值
    if (Indu_R < ad_min[3])
        ad_min[3] = Indu_R;
    else if (Indu_R > ad_max[3])
        ad_max[3] = Indu_R;*/
}
void data_treating(void)
{
    for(uint8 i = 0;i < M;i++)
    {
        data_adc[0][i]=adc_mean_filter(ADC_0,ADC0_CH0_A0,ADC_12BIT,10);
        data_adc[1][i]=adc_mean_filter(ADC_0,ADC0_CH1_A1,ADC_12BIT,10);
        data_adc[2][i]=adc_mean_filter(ADC_0,ADC0_CH5_A5,ADC_12BIT,10);
        data_adc[3][i]=adc_mean_filter(ADC_0,ADC0_CH6_A6,ADC_12BIT,10);
    }
    //************去除最大值和最小值****************//

    for(uint8 i = 0;i < N;i++)
    {
        max_adc_data[i]=min_adc_data[i]=data_adc[i][0];
        for(uint8 j = 0;j < M;j++)
        {
            //  (data_adc[i][j]>=max_adc_data[i])?(max_adc_data[i]=data_adc[i][j]):(max_adc_data[i] = max_adc_data[i]);
            if(data_adc[i][j]>=max_adc_data[i])
                max_adc_data[i]=data_adc[i][j];
            //else
            //    max_adc_data[i] = max_adc_data[i];
            (min_adc_data[i]>=data_adc[i][j])?(min_adc_data[i]=data_adc[i][j]):(min_adc_data[i]=max_adc_data[i]);
        }
    }
    //****************求和********************//
    for(uint8 i = 0;i < N;i++)
    {
        data_sum[i]=0;
        for(uint8 j = 0;j < M;j++)
            data_sum[i]+=data_adc[i][j];
    }

    for(uint8 i = 0;i < N;i++)
    {
        av_adc[i]=(data_sum[i]-min_adc_data[i]-max_adc_data[i])/8;
        data_sum[i]=0;
    }
    //***************滑动滤波********************//
    for(uint8 i = 0;i < M-1;i++)
    {
        data_adc[0][i]=data_adc[0][i+1];
        data_adc[1][i]=data_adc[1][i+1];
        data_adc[2][i]=data_adc[2][i+1];
        data_adc[3][i]=data_adc[3][i+1];
    }
    data_adc[0][M-1]=av_adc[0];
    data_adc[1][M-1]=av_adc[1];
    data_adc[2][M-1]=av_adc[2];
    data_adc[3][M-1]=av_adc[3];
    //****************求滑动的和********************//
    for(uint8 i = 0;i < M;i++)
    {
        data_sum[0] += data_adc[0][i];
        data_sum[1] += data_adc[1][i];
        data_sum[2] += data_adc[2][i];
        data_sum[3] += data_adc[3][i];
    }
    for(uint8 i = 0;i < N;i++)
    {
        av_adc[i]=data_sum[i]/M;
        data_sum[i]=0;
    }

    //****************归一化¯********************//
    need_one[0]=(float)(av_adc[0]-215)/(float)(4095-215);//  500    90 //左1//175
    need_one[1]=(float)(av_adc[1]-294)/(float)(1463-294);//4095  600//左2//1510   196
    need_one[2]=(float)(av_adc[2]-367)/(float)(2224-367);//4095  600//右2//1880   200
    need_one[3]=(float)(av_adc[3]-330)/(float)(4095-330); //1700  980//右1//300
    need =(float) (Indu_M - 1800)/(float)(2300-1800);//中
    for(uint8 i = 0;i < N;i++)
    {
        if(need_one[i]<0.001) need_one[i]=0;
        if(need_one[i]>1) need_one[i]=1;
        need_one[i]*=1000;
    }
}
void elec_servo_PD_control(void)
{
    //sensor = 1;
    // uint8  diuxian;
    float g_Turn_Error_Temp1[2][5];

    elec_redu_in = need_one[3] - need_one[0];
    elec_add_in = need_one[0] + need_one[3];

    cabihe_in = elec_redu_in / elec_add_in;
    cabihe_in = (cabihe_in > 1) ? 1 : cabihe_in;//差比和限幅

    g_Turn_Error_Temp1[0][4] = g_Turn_Error_Temp1[0][3];
    g_Turn_Error_Temp1[0][3] = g_Turn_Error_Temp1[0][2];
    g_Turn_Error_Temp1[0][2] = g_Turn_Error_Temp1[0][1];
    g_Turn_Error_Temp1[0][1] = g_Turn_Error_Temp1[0][0];
    g_Turn_Error_Temp1[0][0] = cabihe_in;

    display_chabihe = cabihe_in * 100;

    g_Turn_Error_dot[0] = 5*(g_Turn_Error_Temp1[0][0]-g_Turn_Error_Temp1[0][1]);        //水平电感的偏差微分
    g_Turn_Error_dot[0] = (g_Turn_Error_dot[0]> 0.7? 0.7:g_Turn_Error_dot[0]);          //偏差微分限幅
    g_Turn_Error_dot[0] = (g_Turn_Error_dot[0]<-0.7?-0.7:g_Turn_Error_dot[0]);

    display_dot = g_Turn_Error_dot[0] * 1000;
    //****************圆环差比和********************//
    elec_redu_out = need_one[2] - need_one[1];
    elec_add_out = need_one[1] + need_one[2];
    chabihe_out = elec_redu_out / elec_add_out;
    chabihe_out = (chabihe_out > 1) ? 1 : chabihe_out;   //差比和限幅

    g_Turn_Error_Temp1[1][4] = g_Turn_Error_Temp1[1][3];
    g_Turn_Error_Temp1[1][3] = g_Turn_Error_Temp1[1][2];
    g_Turn_Error_Temp1[1][2] = g_Turn_Error_Temp1[1][1];
    g_Turn_Error_Temp1[1][1] = g_Turn_Error_Temp1[1][0];
    g_Turn_Error_Temp1[1][0] = chabihe_out;

    g_Turn_Error_dot[1] = 5*(g_Turn_Error_Temp1[1][0]-g_Turn_Error_Temp1[1][3]);                //垂直电感的偏差微分
    g_Turn_Error_dot[1] = (g_Turn_Error_dot[1]> 0.7? 0.7:g_Turn_Error_dot[1]);           //偏差微分限幅
    g_Turn_Error_dot[1] = (g_Turn_Error_dot[1]<-0.7?-0.7:g_Turn_Error_dot[1]);


    //electricity_P = electricity_P_z;//100          //200     //300(3500)   //250
    // electricity_D = electricity_D_z;//90               //20     71(3500)     158

    if((display_chabihe < 10)&&(display_chabihe > -30))
    {
        electricity_P = 150;//100          //200     //300(3500)   //250
        electricity_D = 150;//90               //20     71(3500)     158
        //    dianji_pwm(1179);
        if(Gear == 2)
            dianji_sudu(lang_pwm);
        else if(Gear == 1)
            dianji_sudu(3200);
    }
    /*   else if((myabs(display_chabihe) > 20) && (myabs(display_chabihe)  <= 80))
    {

        electricity_P = 400;//400         //350   //420
        electricity_D = 0;//35         //  0      // 15

       electricity_P = 420;
       electricity_D = 15;
    }*/
    // else if((myabs(display_chabihe) > 20) && (myabs(display_chabihe)  <= 50))
    /*  else
    {
        electricity_P = 900;
        electricity_D = 0;

    }*/
    else //if((display_chabihe < -35) && (display_chabihe  >16))
    {
        //        electricity_P = 250;        //方向控制P//900
        //        electricity_D = 250;         //方向控制D//40
        electricity_P = 250;
        electricity_D = 250;
        //    dianji_pwm(1000);
        dianji_sudu(wan_pwm);
    }
    //    else
    //    {
    //        electricity_P = 1000;        //方向控制P//900
    //        electricity_D = 0;         //方向控制D//40
    //        //   dianji_sudu(2500);
    //    }
    //****************转向环限速********************//
    Object_elec_Turn_Out = cabihe_in * electricity_P + g_Turn_Error_dot[0] * electricity_D;
    Object_elec_Turn_Out_1 = chabihe_out *900 + g_Turn_Error_dot[1] *40;
    Object_elec_Turn_Out_2 = (120*((3*(need_one[2] + need_one[3])) - (need_one[0] + need_one[1])*3))/(need_one[0]+need_one[3]);
    //  Object_elec_Turn_Out_2 = (float)(300*(Indu_RR - Indu_LL)/(Indu_RR+Indu_LL));
    // elec_Turn_Out = Object_elec_Turn_Out;
    //
    if((zhijiao_flag))//直角
    {
        /*        dianji_sudu(2000);
            if(E6 ==0)
            {
                zhijiao_tui();
            }
         //   else
          if((myabs(display_chabihe) > 50))

          else
            zhijiao_tui();*/
        if(zhijiao_kaiguan == 2)
        {
            elec_Turn_Out = Object_elec_Turn_Out_2;
            dianji_sudu(2350);
        }
        else if(zhijiao_kaiguan == 1)
            elec_Turn_Out = Object_elec_Turn_Out;
        // dianji_pwm(566);
        //  elec_Turn_Out = Object_elec_Turn_Out;
    }
    else
    {//pid跑
        elec_Turn_Out = Object_elec_Turn_Out;
        // dianji_sudu(3200);
        // dianji_pwm(1179);
    }
    // podao();
    yuanhuan();
    // zhijiao();
    duoji_pwm(elec_Turn_Out);
    //Transmit_FB(&elec_Turn_Out);
    while((Indu_M < 800)&&((elec_Turn_Out>160)||(elec_Turn_Out < -160)));
    // yuanhuan_fujia();
}

void oled_linshi2()
{
    oled_uint16(0,0,Indu_LL);//最左边电感
    oled_uint16(63,0,Indu_L);//左 竖直电感
    oled_uint16(0,1,Indu_RR);//最右边电感
    oled_uint16(63,1,Indu_R);//由 竖直电感
    oled_uint16(0,2,Indu_M);//中间电感
    oled_uint16(63,2,680+elec_Turn_Out);//舵机打角
    /*    oled_printf_float(0,3,need_one[0],5,5);
    oled_printf_float(0,4,need_one[1],5,5);
    oled_printf_float(0,5,need_one[2],5,5);
    oled_printf_float(0,6,need_one[3],5,5);*/
    oled_uint16(0,3,Indu_LM);//左内八电感
    oled_uint16(63,3,Indu_RM);//右内八电感
    oled_printf_float(0,4,cabihe_in,5,5);
    oled_printf_int32(0,5,mc,10);
    oled_int16(0,6,sudu1);
    oled_uint16(0,7,tingche_adc);
}
uint8  send_num1(uint16 num,uint16* sendbuff)
{
    uint8 i = 0,j = 0;
    uint16 temp;
    while(1)
    {
        if(num<10)
        {
            sendbuff[i++] =  num+'0';
            break;
        }
        sendbuff[i++] =  num%10+'0';
        num/=10;
        if(num<10)
        {
            sendbuff[i++] =  num+'0';
            break;
        }

    }
    for(j=0;j<i/2;j++)
    {
        temp = sendbuff[j];
        sendbuff[j] = sendbuff[i-1-j];
        sendbuff[i-1-j] = temp;
    }

    return i;
}
void wirs1()
{
    // uint16 buff[10],num;
    //  uint16 fa[13];
    /*    fa[0] = need_one[0];
    fa[1] = need_one[1];
    fa[2] = need_one[2];
    fa[3] = need_one[3];
    fa[4] = Indu_LL;
    fa[5] = Indu_L;
    fa[6] = Indu_R;
    fa[7] = Indu_RR;*/
    ANO_DT_send_int16(Indu_M,Indu_L,Indu_R,Indu_LM,Indu_RM,(short)display_chabihe,Indu_LL,Indu_RR);
    //    fa[0] = Indu_M;
    //    fa[1] = Indu_LL;
    //    fa[2] = Indu_RR;
    //    fa[3] = Indu_L;
    //    fa[4] = Indu_R;
    //    fa[5] = display_chabihe;
    //    fa[6] = 680+elec_Turn_Out;
    //    fa[7] = 0X5A;
    //    /*    fa[7] = lrring;
    //    fa[8] = speed4;*/
    //    for(m=0;m<8;m++)
    //    {
    //        num = send_num1(fa[m],buff);
    //        buff[num++] = '\r';
    //        buff[num++] = '\n';
    //        seekfree_wireless_send_buff(buff,num);
    //    }
}
/*圆环数据
 * 右圆环
 *              |         —             —                       —            |
 *   入                                                                 >3000                                                                 4095
 *   出                   4095                                     >2500
 * 左圆环
 *              |         —             —                       —            |
 * 入                    4095                                       >3000
 *  出                                                                   >3000                                                                 4095
 */
//uint16 huanchu_flag1 =0;//出圆环延时 保证车模走过入环点 不会再次入环
uint8 lrring = 0;//判断左侧环右侧环  0默认  1左侧环 2 右侧环
uint8 benhuan_flag = 0;//圆环判断一次 在判断到圆环时赋值为1 入环程序执行完毕后 清零
uint8 qiedianpanduan = 0; //只切点判断一次 避免左环标志刷新掉
sint32 TangentPointpulse = 0; // 脉冲记忆临时变量1
sint32 EnterCirclePulse = 0;  // 脉冲记忆临时变量2
sint32 OutCirclePulse = 0;    // 脉冲记忆临时变量3
//sint32 EnterCircleOKPulse = 0;// 脉冲记忆临时变量4
//uint8 TangentPoint = 1;    // 切点判断   0切点结束；默认1可以入环，读取脉冲为入环准备
//uint8 EnterCircle = 0;     // 允许进环  默认 0不可进环；1可以进环
uint8 OutCircle = 0;       // 出环标志位  避免出环时判断为入环 乱拐方向 出换主要靠pid自行循迹
//uint8 LeftRightCircle = 0; // 左侧环还是右侧环 默认0原始；1左环；2右环
void yuanhuan()
{
    if(benhuan_flag == 0)
    {
        if(Indu_M >2300)
        {
            TangentPointpulse = mc; // 读取当前脉冲数值
            //TangentPoint = 0;             // 禁止再次读取当前脉冲数值
            benhuan_flag = 1;
            qiedianpanduan = 1;
        }
    }
    //避免误判
    if((benhuan_flag ==1)&&(mc > TangentPointpulse +20000)&&(OutCircle == 0))
    {
        benhuan_flag = 0;
    }
    if(qiedianpanduan == 1)
    {
        if((benhuan_flag ==1)&&(Indu_LM >4000))
        {
            lrring = 2;
            EnterCirclePulse = mc;
            qiedianpanduan = 0;
        }
        else if((benhuan_flag ==1)&&(Indu_RM >4000))
        {
            lrring = 1;
            EnterCirclePulse = mc;
            qiedianpanduan = 0;
        }
    }
    if((lrring == 2)&&(mc> EnterCirclePulse +7500))//右圆环
    {
        elec_Turn_Out =  110;
        duoji_pwm(elec_Turn_Out);
        systick_delay_ms(STM0,300);
        EnterCirclePulse = 10000000;
        lrring =0;
        // benhuan_flag = 0;
        EnterCirclePulse = 0;
        OutCircle = 1;
        OutCirclePulse = mc;
        TangentPointpulse = 0;
    }
    else if((lrring == 1)&&(mc> EnterCirclePulse +4500))//左圆环
    {
        elec_Turn_Out = -110;
        duoji_pwm(elec_Turn_Out);
        systick_delay_ms(STM0,300);
        EnterCirclePulse = 10000000;
        lrring =0;
        //benhuan_flag = 0;
        EnterCirclePulse = 0;
        OutCircle = 1;
        OutCirclePulse = mc;
        TangentPointpulse = 0;
    }
    if ((OutCircle == 1)&&(mc > OutCirclePulse + 160000))//一米的圆环是160000  一米五的圆环2400000   一米二的圆环是200000
    {
        benhuan_flag  = 0;
        OutCircle = 0;
    }
    //    if(benhuan_flag ==0)
    //    {
    //        if(Indu_M > 2500)
    //        {
    //            if (TangentPoint)
    //            {
    //                TangentPointpulse = mc; // 读取当前脉冲数值
    //                TangentPoint = 0;             // 禁止再次读取当前脉冲数值
    //                benhuan_flag = 1;
    //            }
    //        }

    //        if(Indu_LM == 4095)
    //        {
    //            lrring =2;
    //            benhuan_flag = 1;
    //        }
    //        else if(Indu_RM ==4095)
    //        {
    //            lrring = 1;
    //            benhuan_flag = 1;
    //        }
    //    }
    //    if((Indu_LM ==4095)&&(lrring ==1))
    //    {
    //        elec_Turn_Out = 160;
    //        duoji_pwm(elec_Turn_Out);
    //        systick_delay_ms(STM0,500);
    //        lrring =0;
    //        benhuan_flag = 0;
    //    }
    //    else if((Indu_RM ==4095)&&(lrring ==2))
    //    {
    //        elec_Turn_Out = - 160;
    //        duoji_pwm(elec_Turn_Out);
    //        systick_delay_ms(STM0,500);
    //        lrring =0;
    //        benhuan_flag = 0;
    //    }
    //    if(benhuan_flag ==0)
    //    {
    //    if(Indu_M > 2500)
    //    {
    //    if(Indu_R > 4000)
    //        lrring = 2;
    //    else if(Indu_L > 4000)
    //        lrring = 1;
    //   // dianji_sudu(3000);
    //    }
    //    }
    //   //f((Indu_LL ==4095)&&(Indu_R >4000)&&(Indu_RM >=3800))//右圆环
    //    if((Indu_LL >4000)&&(Indu_LM >4000)&&(lrring ==2))
    //    {
    //       // lrring = 1;
    //        //dianji_sudu(3500);
    //        // dianji_pwm(1438);
    //        elec_Turn_Out = 130;
    //        duoji_pwm(elec_Turn_Out);
    //        //    elec_Turn_Out = Object_elec_Turn_Out_1;
    //        systick_delay_ms(STM0,500);
    //        benhuan_flag = 0;
    //        lrring = 0;
    //    }
    //   // else if((Indu_RR ==4095)&&(Indu_L > 4000)&&(Indu_LM >4000))//左圆环
    //    else if((Indu_RR > 4000)&&(Indu_RM >4000)&&(lrring ==1))
    //    {
    //       // lrring = 2;
    //        //dianji_sudu(3500);
    //        //dianji_pwm(1438);
    //        elec_Turn_Out = -130;
    //        duoji_pwm(elec_Turn_Out);
    //        //    elec_Turn_Out = Object_elec_Turn_Out_1;
    //        systick_delay_ms(STM0,500);
    //        benhuan_flag = 0;
    //        lrring = 0;
    //    }
    /*if((lrring == 1)&&(Indu_M >3000)&&(Indu_LM > 4000)&&(Indu_L > 4000)) //左侧环
    {
      elec_Turn_Out = -160;
    //    elec_Turn_Out = Object_elec_Turn_Out_1;
      systick_delay_ms(STM0,500);
      huan_flag = 1;
    }*/
    /*else if((lrring == 2)&&(Indu_M >3000)&&(Indu_RM > 3500)&&(Indu_R > 1000)) //右侧环
    {
      elec_Turn_Out = 160;
    //    elec_Turn_Out = Object_elec_Turn_Out_1;
      systick_delay_ms(STM0,500);
      huan_flag = 2;
    }*/
    //出环程序
    /* if((huan_flag==1)&&(Indu_M >3000)&&(Indu_LM > 4000)&&(Indu_L > 4000))//出环程序 左圆环
    {
        elec_Turn_Out = Object_elec_Turn_Out;
        systick_delay_ms(STM0,500);
        huan_flag = 0;
      //  huanchu_flag1 =1;
    }
    else if((huan_flag==2)&&(Indu_M >3000)&&(Indu_RM > 4000)&&(Indu_R > 4000))//右圆环 出环程序
    {
        elec_Turn_Out = Object_elec_Turn_Out;
        systick_delay_ms(STM0,500);
        huan_flag = 0;
        //huanchu_flag1 = 2;
    }*/
}
void yuanhuan_fujia()
{

    if(huan_flag ==1)
    {
        while((Indu_M > 2300)||(Indu_M <2000));
        lrring = 0;
        //   systick_delay_ms(STM0,300);
        // huan_flag = 0;
        huanchu_flag =1;
    }
    else if (huan_flag ==2)
    {
        while((Indu_M > 2300)||(Indu_M <2000));
        lrring = 0;
        // systick_delay_ms(STM0,300);
        //  huan_flag = 0;
        huanchu_flag =2;
    }
    //出环程序
    /*    if(huanchu_flag1==1||huanchu_flag1==2)//出环延时 保证车模不会再次入环
    {
        systick_delay_ms(STM0,500);
        huanchu_flag1 =0;
        huanchu_flag = 0;
    }*/
}
void zhijiao_tui()//直角后退算法
{
    //  duoji_pwm(0);
    uint8 zjdir =0;
    uint8  dajiao;
    sint32 mc1;//记录倒车时编码器值
    uint8 jizhi_flag =0;
    //  if(Indu_M < 1200)
    //   {
    if(Indu_L > 4000)//(((Indu_L - Indu_R)> 1500) )
    {
        zjdir = 1;//左直角
        jizhi_flag = 1;
    }
    else if(Indu_R > 4000)//(((Indu_R - Indu_L)>1500) )
    {
        zjdir = 2;//右直角
        jizhi_flag = 1;
    }
    //  }
    if(zjdir == 1 || zjdir == 2)
    {
        if(jizhi_flag ==1)
        {
            mc1 = mc;
            jizhi_flag = 0;
        }
        while(mc > mc1-7000)
        {
            duoji_pwm(0);
            pwm_duty(ATOM2_CH2_P11_3,0);
            pwm_duty(ATOM2_CH1_P11_2,3000);
            dajiao = 1;
        }
    }
    mc1 = mc;
    if(dajiao ==1)
    {
        while(mc < mc1+25000)
        {
            if(zjdir ==1)
                duoji_pwm(-160);
            else if(zjdir ==2)
                duoji_pwm(160);
            // dianji_pwm(150);
            pwm_duty(ATOM2_CH2_P11_3,2500);
            pwm_duty(ATOM2_CH1_P11_2,0);
            /*  if(Indu_M >1500)
        break;*/
            //systick_delay_ms(STM0,600);
        }
        //读出车回直角是中间电感的值 写入
        dajiao =0;
        zjdir = 0;
    }

}
void ai_data()
{
    ad_data[0] = (int16)adc_mean_filter(ADC_0,ADC0_CH0_A0,ADC_8BIT,10) - 128;
    ad_data[1] = (int16)adc_mean_filter(ADC_0,ADC0_CH1_A1,ADC_8BIT,10) - 128;
    ad_data[2] = (int16)adc_mean_filter(ADC_0,ADC0_CH2_A2,ADC_8BIT,10) - 128;
    ad_data[3] = (int16)adc_mean_filter(ADC_0,ADC0_CH3_A3,ADC_8BIT,10) - 128;
    ad_data[4] = (int16)adc_mean_filter(ADC_0,ADC0_CH4_A4,ADC_8BIT,10) - 128;
    ad_data[5] = (int16)adc_mean_filter(ADC_0,ADC0_CH5_A5,ADC_8BIT,10) - 128;
    ad_data[6] = (int16)adc_mean_filter(ADC_0,ADC0_CH6_A6,ADC_8BIT,10) - 128;
    ad_data1[0] = adc_mean_filter(ADC_0,ADC0_CH0_A0,ADC_12BIT,10);
    ad_data1[1] = adc_mean_filter(ADC_0,ADC0_CH1_A1,ADC_12BIT,10);
    ad_data1[2] = adc_mean_filter(ADC_0,ADC0_CH2_A2,ADC_12BIT,10);
    ad_data1[3] = adc_mean_filter(ADC_0,ADC0_CH3_A3,ADC_12BIT,10);
    ad_data1[4] = adc_mean_filter(ADC_0,ADC0_CH4_A4,ADC_12BIT,10);
    ad_data1[5] = adc_mean_filter(ADC_0,ADC0_CH5_A5,ADC_12BIT,10);
    ad_data1[6] = adc_mean_filter(ADC_0,ADC0_CH6_A6,ADC_12BIT,10);
    ai_data_flag = 1;
}
void ai_wirs()
{
    ai_elec_turn_out = (int16)limit(elec_Turn_Out,165);
    if(ai_data_flag)
    {
        ai_send_buff[0] = 0;
        ai_send_buff[1] = 0;
        ai_send_buff[2] = ad_data[0];
        ai_send_buff[3] = ad_data[1];
        ai_send_buff[4] = ad_data[2];
        ai_send_buff[5] = ad_data[3];
        ai_send_buff[6] = ad_data[4];
        ai_send_buff[7] = ad_data[5];
        ai_send_buff[8] = ad_data[6];
        ai_send_buff[9] = (uint8)((int32)ai_elec_turn_out*128/161);
        ai_send_buff[10] = 0x5a;
        seekfree_wireless_send_buff1(ai_send_buff,11);
    }
}
void oled_linshi3()
{
    oled_int16(0,0,ad_data[0]);oled_uint16(63,0,ad_data1[0]);
    oled_int16(0,1,ad_data[1]);oled_uint16(63,2,ad_data1[1]);
    oled_int16(0,2,ad_data[2]);oled_uint16(63,3,ad_data1[2]);
    oled_int16(0,3,ad_data[3]);oled_uint16(63,4,ad_data1[3]);
    oled_int16(0,4,ad_data[4]);oled_uint16(63,5,ad_data1[4]);
    oled_int16(0,5,ad_data[5]);oled_uint16(63,6,ad_data1[5]);
    oled_int16(0,6,ad_data[6]);oled_uint16(63,7,ad_data1[6]);
}
void podao()
{    sint32 speed1;
     uint8   pd;
    if(podao_flag)
    {
      speed1 = mc;
      pd = 1;
    }
    if((pd==1)&&(lrring !=0))
        pd = 0;
    if((Indu_M <1800) &&(mc<(speed1+15000))&&(pd == 1))
    {
       elec_Turn_Out = Object_elec_Turn_Out;
       duoji_pwm(elec_Turn_Out);
       while(Indu_M >2000);
       pd = 0;
    }
}
sint16 sudu_linshi;
void sudu_control()
{
    //    if(!zhijiao_flag)
    //    {
    //    if((display_chabihe < 16)&&(display_chabihe > -35))
    //        dianji_pwm(1200);
    //    else
    //        dianji_pwm(1000);
    //    }
    if(jiajiansu == 1)
    {
    if(sudu1 < 100)
    {
        //  systick_delay_ms(STM0,15);
        if(sudu1<100)
        {
            dianji_sudu(3800);
            // systick_delay_ms(STM0,50);
        }
    }
    else if ((sudu_linshi - sudu1) >  1000)
    {
        // systick_delay_ms(STM0,15);
        if((sudu_linshi - sudu1) >  1000)
        {
            dianji_sudu(0);
            //  systick_delay_ms(STM0,50);
        }
    }

    sudu_linshi = sudu1;
    }
}
void zhijiao()
{
    if((Indu_L > 3500)&&((myabs(display_chabihe) < 20))&&(Indu_M < 2500))
    {
        elec_Turn_Out = -165;
        systick_delay_ms(STM0,500);
    }
    else if((Indu_R > 3500)&&((myabs(display_chabihe) < 20))&&(Indu_M <2500))
    {
        elec_Turn_Out = 165;
        systick_delay_ms(STM0,500);
    }
}

void tingche()
{
    if((tingche_adc >=650)&&ting==1)
    {
        //        dianji_sudu(2000);
        //        systick_delay_ms(STM0,400);
        dianji_sudu(0);
        //systick_delay_ms(STM0,60000);
        while(1);
    }
}
